#include "physicsmanager.h"

#include <OgreBullet/Collisions/Utils/OgreBulletCollisionsMeshToShapeConverter.h>
#include <OgreBullet/Collisions/Shapes/OgreBulletCollisionsTrimeshShape.h>
#include <OgreBullet/Collisions/Shapes/OgreBulletCollisionsCylinderShape.h>
#include <OgreBullet/Collisions/Shapes/OgreBulletCollisionsConvexHullShape.h>
#include <OgreBullet/Collisions/Shapes/OgreBulletCollisionsBoxShape.h>
#include <OgreBullet/Collisions/OgreBulletCollisionsShape.h>
#include <LinearMath/btVector3.h>
#include <OgreFrameListener.h>


PhysicsManager *PhysicsManager::m_Instance = 0;

PhysicsManager *PhysicsManager::CreateInstance(Ogre::SceneManager *p_SceneManager)
{
    if (m_Instance == 0)
    {
        m_Instance = new PhysicsManager(p_SceneManager);
    }
    return m_Instance;
}

void PhysicsManager::RemoveAndDestroyChildren()
{
    // OgreBullet physic delete - RigidBodies
    std::deque<OgreBulletDynamics::RigidBody *>::iterator itBody = m_Bodies.begin();
    while (itBody != m_Bodies.end())
    {
       delete *itBody;
       ++itBody;
    }

    // OgreBullet physic delete - Shapes
    std::deque<OgreBulletCollisions::CollisionShape *>::iterator itShape = m_Shapes.begin();
    while (itShape != m_Shapes.end())
    {
       delete *itShape;
       ++itShape;
    }

    m_Bodies.clear();
    m_Shapes.clear();
}

PhysicsManager::PhysicsManager(Ogre::SceneManager *p_SceneManager)
{
	m_SceneManager = p_SceneManager;

	// Start Bullet
	m_DynamicsWorld = new OgreBulletDynamics::DynamicsWorld(m_SceneManager,
															Ogre::AxisAlignedBox (Ogre::Vector3 (-10000, -10000, -10000), Ogre::Vector3 (10000,  10000,  10000)),
															Ogre::Vector3(0, 0, 0));

    bool debug = true;
    if (debug)
    {
        // Add a debug drawer
        m_DebugDrawer = new OgreBulletCollisions::DebugDrawer();
        m_DebugDrawer->setDrawWireframe(false);
        m_DynamicsWorld->setDebugDrawer(m_DebugDrawer);
        m_DynamicsWorld->setShowDebugShapes(false);
        m_DynamicsWorld->setShowDebugContactPoints(false);
        Ogre::SceneNode *node = m_SceneManager->getRootSceneNode()->createChildSceneNode("debugDrawer", Ogre::Vector3::ZERO);
        node->attachObject(static_cast <Ogre::SimpleRenderable *> (m_DebugDrawer));
    }
}

PhysicsManager::~PhysicsManager()
{

    RemoveAndDestroyChildren();
	delete m_DynamicsWorld->getDebugDrawer();
	m_DynamicsWorld->setDebugDrawer(0);
	delete m_DynamicsWorld;
}

void PhysicsManager::AddStaticEntity(Ogre::Entity *p_Entity)
{
	OgreBulletCollisions::StaticMeshToShapeConverter *Converter = new OgreBulletCollisions::StaticMeshToShapeConverter(p_Entity);
	OgreBulletCollisions::CollisionShape *Shape = Converter->createTrimesh();
	//OgreBulletCollisions::CollisionShape *Shape = Converter->createBox();

    OgreBulletDynamics::RigidBody *Body = new OgreBulletDynamics::RigidBody(p_Entity->getName(), m_DynamicsWorld);
    //Body->setStaticShape(m_SceneManager->getRootSceneNode(), Shape, 0.0f, 0.0f);
    Body->setStaticShape(p_Entity->getParentSceneNode(), Shape, 0.0f, 0.0f);

	m_Bodies.push_back(Body);
	m_Shapes.push_back(Shape);
}


void PhysicsManager::AddMovableEntity(Ogre::Entity *p_Entity)
{
    OgreBulletCollisions::StaticMeshToShapeConverter *Converter = new OgreBulletCollisions::StaticMeshToShapeConverter(p_Entity);
    OgreBulletCollisions::CollisionShape *Shape = Converter->createConvex();
	//OgreBulletCollisions::CollisionShape *Shape = Converter->createBox();

    OgreBulletDynamics::RigidBody *Body = new OgreBulletDynamics::RigidBody(p_Entity->getName(), m_DynamicsWorld);
    Body->setShape(p_Entity->getParentSceneNode(), Shape, 0.0f, 0.0f, 0.0f);

	m_Bodies.push_back(Body);
	m_Shapes.push_back(Shape);
}

void PhysicsManager::Collide()
{
	//m_DynamicsWorld->discreteCollide();
    //m_DynamicsWorld->stepSimulation(0.03);
    m_DynamicsWorld->getBulletDynamicsWorld()->stepSimulation(0.1);
    m_DynamicsWorld->getBulletDynamicsWorld()->performDiscreteCollisionDetection();

	int a  = m_DynamicsWorld->getBulletDynamicsWorld()->getDispatcher()->getNumManifolds();
    //if (a > 0 || true)
        //printf("%i Objects colliding\n", a);
}
